"""
Code adapted to python by Konstantinos Sideris Email:ksideris@live.com 

from 

GWTLib(Gabor Wavelets Transform for Face Recognition) 

LICENSE

This software is freely available for non-commercial use such as research and education. 
Please see the full disclaimer below. 

We recommand you cite the reference given below in your publications related to this work.. 

Jianke Zhu, Mang I Vai and Peng Un Mak, "A New Enhanced Nearest Feature Space (ENFS) 
Classifier for Gabor Wavelets Features-Based Face Recognition", ICBA 2004. Lecture Notes in
Computer Science 3072, Springer. pp. 124 - 131.

	
Copyright (c) 2003-2005 Jackie, Jianke Zhu.  Email:jianke.zhu@gmail.com

http://www.cse.cuhk.edu.hk/~jkzhu

"""
"""
 PATENT Warning. The George Mason University has applied for a patent on the Gabor Wavelets transform feature extraction method for face recognition. Commercial applications of this 
software may require a license from the George Mason University.

3RD PART SOFTWARE

The software is partly based on the following libraries:

  - The Intel(tm) OpenCV Library
 

DISCLAIMER

This software is provided 'as-is', without any express or implied warranty.
In no event will the author be held liable for any damages arising from the
use of this software.

Permission is granted to anyone to use this software for any non-commercial 
purpose, and to alter it, subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim
   that you wrote the original software. 

2. Altered source versions must be plainly marked as such, and must not be 
   misrepresented as being the original software.

3. This notice may not be removed or altered from any source distribution.

--

No guarantees of performance accompany this software, nor is any 
responsibility assumed on the part of the author.

This software is provided by Jianke Zhu ``as is'' and any express or implied
warranties, including, but not limited to, the implied warranties of merchan-
tability and fitness for a particular purpose are disclaimed. In no event shall  
Jianke Zhu be liable for any direct, indirect, incidental, special, exemplary,
or consequential damages (including, but not limited to, procurement of substitute
goods or services; loss of use, data, or profits; or business interruption) however 
caused and on any theory of liability, whether in contract, strict liability, 
or tort (including negligence or otherwise) arising in any way out of the use 
of this software, even if advised of the possibility of such damage.


"""



import cv
import numpy as np
import math

def cv2array(im):
    depth2dtype = {
    cv.IPL_DEPTH_8U: 'uint8',
    cv.IPL_DEPTH_8S: 'int8',
    cv.IPL_DEPTH_16U: 'uint16',
    cv.IPL_DEPTH_16S: 'int16',
    cv.IPL_DEPTH_32S: 'int32',
    cv.IPL_DEPTH_32F: 'float32',
    cv.IPL_DEPTH_64F: 'float64',
    }

    arrdtype=im.depth
    a = np.fromstring(
        im.tostring(),
        dtype=depth2dtype[im.depth],
        count=im.width*im.height*im.nChannels)
    a.shape = (im.height,im.width,im.nChannels)
    return a

def array2cv(a):
    dtype2depth = {
    'uint8':   cv.IPL_DEPTH_8U,
    'int8':    cv.IPL_DEPTH_8S,
    'uint16':  cv.IPL_DEPTH_16U,
    'int16':   cv.IPL_DEPTH_16S,
    'int32':   cv.IPL_DEPTH_32S,
    'float32': cv.IPL_DEPTH_32F,
    'float64': cv.IPL_DEPTH_64F,
    }
    try:
        nChannels = a.shape[2]
    except:
        nChannels = 1
    cv_im = cv.CreateImageHeader((a.shape[1],a.shape[0]),
        dtype2depth[str(a.dtype)],
        nChannels)
    cv.SetData(cv_im, a.tostring(),
                a.dtype.itemsize*nChannels*a.shape[1])
    return cv_im

"""
  @python-author Konstantinos Sideris
  @author   ZhuJianke
  @version  07-26-2011
 
  @memo     Save the numpy array to an bmp image
  @doc      Save the numpy array  to an bmp image

  @param    mat      numpy array  
  @param    name	  file name
      
  @return   status
"""
def Save2im(array,coutfilename):
    #array = array * 255.0/(array.max()- array.min())	
    maxVal = array.max()
    minVal = array.min()
    cvim = array2cv(array)
    img = cv.CreateImage(cv.GetSize(cvim),8,1)
    cv.ConvertScale(cvim,img,255.0/(maxVal-minVal),-minVal*255.0/(maxVal-minVal))
    cv.SaveImage(coutfilename,img)	
    return 1

def  GetMat( fh,  isVector ):

    data = fh.read(8)
    Dim = np.fromstring(data, dtype=np.float32)

    rows=0;
    cols=0;
    if(not isVector):
        rows = int(Dim[0]) #Save the dimensions of the matrix.
        cols = int(Dim[1]) 
    else:
        cols = int(Dim[0]) #Save the dimensions of the matrix.
        rows = int(Dim[1])


    data = fh.read(rows*cols*4)
    Elem = np.fromstring(data, dtype=np.float32)

    m  = cv.CreateMat(rows,cols,cv.CV_64FC1);	
# cv.CreateData( m);
    cIndex=0
    for cI in range(rows): #//Put the elements into the matrix.
        for cJ in range(cols):
            cv.Set2D(m,cI,cJ,float(Elem[cIndex]));
            cIndex+=1;

    return m

"""
  @python-author Konstantinos Sideris

  @version  07-26-2011
 
  @memo     Calculate the zero mean unit length of a vector.
  @doc       Calculate the zero mean unit length of a vector.

  @param    v		  input vector. 
  
  @return   nothing
"""
def ZeroMeanUnitLength(v) :      

    mean = v.mean()
    sqsum = 0
    for i  in range(len(v)):
        v[i]=v[i]#-mean
        sqsum= sqsum+math.pow(v[i],2)
    a=1.0
    if(sqsum>0):
        a = 1.0/math.sqrt(sqsum);

    return v*a   

def WriteMat(m,fh,isVector ): 

    #FILE *Stream;					#Stream for output.
    cIndex=2;					#Counter for elements in the matrix.
    cSize=m.shape[0] *m.shape[1]+2;	#The number of elements to save.
    #double*Elem;					#Pointer to all the element to save.

    #assert( fh!=NULL );
    Stream = fh;

    Elem=np.zeros(cSize,np.float32) 


    if(not isVector):
        Elem[0]=m.shape[0];#Save the dimensions of the matrix.
        Elem[1]=m.shape[1];
    else:
        Elem[0]=1.0;#//Save the dimensions of the matrix.
        Elem[1]=m.shape[0];
      
        #int cI,cJ;
        for cI in range(m.shape[0]):
            for cJ in range(m.shape[1]):
                Elem[cIndex]=cv.Get2D( m,cI,cJ)[0]
                cIndex+=1

    fh.write(Elem)


def LoadGaborFFT(fldname):				

    #mGabor=np.#(CvMat**)malloc(40*sizeof(CvMat));
  
    filename = fldname+'/FFTgabor_'+str(0) + '_'+str(0)+'.data'

    ifp = open(filename, 'rb');
    mGabor= [GetMat(ifp,False)]*40;


    ifp.close();
    for i in range(5):
        for j in range(8):
            filename = fldname+'/FFTgabor_'+str(i) + '_'+str(j)+'.data'
            ifp = open(filename, 'rb');
            mGabor[i*8+j]	= GetMat(ifp,False);

            ifp.close();		

    return(mGabor);

def avg2(gout,point,area):
    _sum=0
    for i in range(-area/2+point[0],area/2+point[0]):
        for j in range(-area/2+point[1],area/2+point[1]):
            _sum	+= math.pow(gout[i,j],2)

    return _sum/(area*area)

def avg(gout,point,area):
    sub = gout[point[0]-area/2:point[0]+area/2,point[1]-area/2:point[1]+area/2]
    return (sub**2).mean()

def gabor_extraction2(img,_object,mGabor,features):

    w=128;h=128;
    img_size = cv.GetSize( img );
    imtmp=cv.CreateImage(img_size,cv.IPL_DEPTH_64F,0);	

    cv.ConvertScale(img,imtmp,1.0,0);
 
    dft_M = cv.GetOptimalDFTSize( w+h- 1 );
    dft_N = cv.GetOptimalDFTSize( w+h- 1);
    imdft=cv.CreateMat(dft_M, dft_N, cv.CV_64FC1 );
  
    cv.Zero(imdft);
    for i in range(h):
        for j in range(w):
            cv.Set2D(imdft,i,j,cv.Get2D(imtmp,i,j)[0])

    cv.DFT( imdft, imdft, cv.CV_DXT_FORWARD, w );
    n=w*h/64;
    _index=np.zeros(_object.shape,int)

    for i in range(5):
        for j in range(8):		
            gout=cv.CreateMat(dft_M, dft_N, cv.CV_64FC1 );
            cv.MulSpectrums( imdft,mGabor[i*8+j], gout, 0);		
            cv.DFT( gout, gout, cv.CV_DXT_INV_SCALE,w+h-1);  		
            nm=str(i)+'_'+str(j)+'.bmp';

            #Save2im(np.asarray( gout[:,:] ),nm);

            _gout = np.asarray( gout[:,:] )
            for f in range(len(features)):	
                point = (int(round(features[f][1])),int(round(features[f][2])))			
                temp = math.pow(avg(_gout,point,10),2) #math.pow(_gout[] ,2)+math.pow(_gout[round(features[f][1]),round(features[f][2])+1] ,2)
                _object[f,i*8+j] = math.sqrt(temp)

    for f in range(len(features)):	
        #_object[f]  *= 1.0/np.sum(_object[f] **2)
        _object[f]=ZeroMeanUnitLength(_object[f]);

    return _object;

def extract_features2( img,mGabor,features):
    #print 'starting extract_features'
    nsize = 40*128*128/64;
    size = (128,128);	
    img_size = cv.GetSize( img );
    ipl=	cv.CreateImage(img_size,8,0);
    if(img.nChannels==3):
        cv.CvtColor(img,ipl,cv.CV_BGR2GRAY);
    else:
        cv.Copy(img,ipl,0);

    if((size[0]!=img_size[0]) or (size[1]!=img_size[1])):

        tmpsize=cv.CreateImage(size,8,0);	
        cv.Resize(ipl,tmpsize,cv.CV_INTER_LINEAR);

        ipl=cv.CreateImage(size,8,0);	
        cv.Copy(tmpsize,ipl);


        _object=np.zeros((len(features),40),np.float32);
        tmp=cv.CreateImage(size,cv.IPL_DEPTH_64F,0);	

        cv.ConvertScale(ipl,tmp,1.0,0);	


    #Gabor wavelet
    _object =gabor_extraction2(tmp,_object,mGabor,features);
    #  _object = ZeroMeanUnitLength(_object);
    return _object;
  
def gabor_extraction(img,_object,mGabor):

    w=128;h=128;
    img_size = cv.GetSize( img );
    imtmp=cv.CreateImage(img_size,cv.IPL_DEPTH_64F,0);	

    cv.ConvertScale(img,imtmp,1.0,0);

    # coutfilename=(char*)malloc(512*sizeof(char));
  
    dft_M = cv.GetOptimalDFTSize( w+h- 1 );
    dft_N = cv.GetOptimalDFTSize( w+h- 1);
    imdft=cv.CreateMat(dft_M, dft_N, cv.CV_64FC1 );
      
    cv.Zero(imdft);
    for i in range(h):
        for j in range(w):
            cv.Set2D(imdft,i,j,cv.Get2D(imtmp,i,j)[0])		
    cv.DFT( imdft, imdft, cv.CV_DXT_FORWARD, w );
    n=w*h/64;
    _index=np.zeros(_object.shape,int)


    
    for i in range(5):
        for j in range(8):		
            gout=cv.CreateMat(dft_M, dft_N, cv.CV_64FC1 );
            #cv.ShowImage('a',mGabor[i*8+j])			

            cv.MulSpectrums( imdft,mGabor[i*8+j], gout, 0);		
            cv.DFT( gout, gout, cv.CV_DXT_INV_SCALE,w+h-1);  		

            nm=str(i)+'_'+str(j)+'.bmp';

            #result = cv2array(gout)
            #print 'result',gout.shape
            
            #cv.ShowImage('a',gout)
            #cv.WaitKey(0)
            #Save2im(np.asarray( gout[:,:] ),nm);
            #print w,h,cv.GetSize(gout)
            #print 'gout',cv.GetSize(gout)
            for x in range(4,w/2,8) :
                for y in range(4,h/2,8) :
                    r1 = cv.Get2D(gout, (x+w/2),((y+h/2)*2))
                    r2 = cv.Get2D(gout, (x+w/2),((y+h/2)*2+1))			
                          
                    _sum=math.pow(r1[0],2) + math.pow(r2[0] ,2) 
                    _object[(i*8+j)*n+x/8*h/8+y/8]=math.sqrt(_sum);

                    #		_index[(i*8+j)*n+x/8*h/8+y/8] = (i*8+j)*n+x/8*h/8+y/8
            
                #_object = ZeroMeanUnitLength(_object);
                        
            
                #print _index
                #print _object;
    
    
    #print _object;
    return _object;

def extract_features( img,mGabor):
    #print 'starting extract_features'
    nsize = 40*128*128/64;
    size = (128,128);	
    img_size = cv.GetSize( img );
    ipl=	cv.CreateImage(img_size,8,0);
    if(img.nChannels==3):
        cv.CvtColor(img,ipl,cv.CV_BGR2GRAY);
    else:
        cv.Copy(img,ipl,0);
   
    if((size[0]!=img_size[0]) or (size[1]!=img_size[1])):

        tmpsize=cv.CreateImage(size,8,0);	
        cv.Resize(ipl,tmpsize,cv.CV_INTER_LINEAR);

        ipl=cv.CreateImage(size,8,0);	
        cv.Copy(tmpsize,ipl);



    _object=np.zeros(nsize,np.float32);
    tmp=cv.CreateImage(size,cv.IPL_DEPTH_64F,0);	

    cv.ConvertScale(ipl,tmp,1.0,0);	
 

    #Gabor wavelet
    _object =gabor_extraction(tmp,_object,mGabor);
 
    #  _object = ZeroMeanUnitLength(_object);

    return _object,nsize;


def gabor_kernel(scale,orientation,mask_size,kmax,sigma,filename):

    gabor_cos = np.zeros((mask_size*mask_size),np.float32)
    gabor_sin = np.zeros((mask_size*mask_size),np.float32)

    offset = mask_size / 2;

    f=math.sqrt(2.0);
    sig=sigma*sigma;
    for v in range(scale): #(v=0;v<scale;v++)
        for u in range(orientation):#(u=0;u<orientation;u++) {
            kv=kmax/math.pow(f,v);
            phiu=u*math.pi/8.0;
            kv_mag=kv*kv;
            for x in range(mask_size): # (x = 0; x < mask_size; x++) 
                for y in range(mask_size): #(y = 0; y< mask_size; y++) {
                    i=x-offset; 
                    j=y-offset;
                    mag=float(i*i+j*j);
                    gabor_cos[x*mask_size+y] = kv_mag/sig*math.exp(-0.5*kv_mag*mag/sig)*\
                                    (math.cos(kv*(i*math.cos(phiu)+j*math.sin(phiu)))-math.exp(-1.0*sig/2.0))
                    gabor_sin[x*mask_size+y] = kv_mag/sig*math.exp(-0.5*kv_mag*mag/sig)*\
                                    (math.sin(kv*(i*math.cos(phiu)+j*math.sin(phiu))));
                    #}

        # Let's deal with the cosine part first */

    coutfilename  = 'gabor/C'+filename+'_'+str(v)+'_'+str(u) #+'.data'
    soutfilename  = 'gabor/S'+filename+'_'+str(v)+'_'+str(u)#+'.data'

 
    cout = open(coutfilename+'.data', 'wb')
    #print "Name of the file: ", cout.name

    WriteMat(gabor_cos.reshape(mask_size,mask_size),cout,False);
    
    cout.close()
    Save2im(gabor_cos.reshape(mask_size,mask_size),coutfilename+'.bmp')
    # Now let's deal with the sine part 

    sout = open(soutfilename+'.data', 'wb')
    #print "Name of the file: ", sout.name
    
    WriteMat(gabor_sin.reshape(mask_size,mask_size),sout,False);
    
    sout.close()
    Save2im(gabor_sin.reshape(mask_size,mask_size),soutfilename+'.bmp')

def gabor_filter( base_fld):

    img_size=128;
    #coutfilename
    print 'Building gabor filter bank data: '
    gabor_kernel(5,8,img_size,math.pi/2.0,2*math.pi, 'gabor'); 
    print 'OK!'
    print 'Calculating Gabor filter bank FFT:'
       
    dft_M = cv.GetOptimalDFTSize( img_size + img_size - 1 );
    dft_N = cv.GetOptimalDFTSize( img_size + img_size - 1 );
    for i in range(5):
        for j in range(8):
            coutfilename = 'gabor/Cgabor'+'_'+str(i)+'_'+str(j)+'.data';
            ifp = open(coutfilename, "rb");
            mGaborReal	= GetMat(ifp,False);
            ifp.close();
            soutfilename = 'gabor/Sgabor'+'_'+str(i)+'_'+str(j)+'.data';
            ifps = open(soutfilename, "rb");
            mGaborImg	= GetMat(ifps,False);
            ifps.close();
             
            mGabor=cv.CreateMat( dft_M, dft_N, cv.CV_64FC1 );		
                                
            mGaborFFT=cv.CreateMat( dft_M, dft_N, cv.CV_64FC1);      		

            for x in range(img_size):
                for y in range(img_size):
                    elemR = cv.Get2D(mGaborReal,x,y)
                    cv.Set2D(mGabor,x,y,elemR[0])
                    elemI = cv.Get2D(mGaborImg,x,y)
                    cv.Set2D(mGabor,x,y*2+1,elemI[0])

            cv.DFT(mGabor,mGaborFFT,cv.CV_DXT_FORWARD,img_size);
        
            coutfilename= ''+ base_fld+'/FFTgabor'+'_'+str(i)+'_'+str(j)+'.data';
            ofps = open(coutfilename, "wb");
            WriteMat(np.asarray( mGaborFFT[:,:] ),ofps,False);
        
            ofps.close();


    print 'OK!';
    return 1;

def writeData(v,length, fh,total):

    Elem= np.zeros( (length+2)*4 ,np.float32)
    print length
    Elem[0]=np.float32(total)
    Elem[1]=np.float32(length);
    #print 'length', Elem[1]
    
    for i in range(length):
        Elem[i+2]=v[i]
  
    fh.write(Elem)
  
def writeDataNoHeader(v,length, fh):

    Elem= np.zeros( (length+2)*4 ,np.float32)	
    
    for i in range(length):
        Elem[i]=v[i]
    
    fh.write(Elem)
  

"""
  @author   Zhu Jianke
  @python_port 
	    Konstantinos Sideris
  @version  9-11-2003

  @memo     Writes double array into file binary mode.
  @doc      Writes double array into file binary mode.
  @see		ReadData
  
  @param    v		    Double array pointer.
  @param    length	    The length of the v.
  @param	fh			File handler.

  @return   true on success, false on file errors.
"""
def getData(fh):

    data = fh.read(8)
    Dim = np.fromstring(data, dtype=np.float32)
    
    total =  int(Dim[0]);
    length=int(Dim[1]);
    
    print 'length', length ,'total',total
    data = fh.read(length*total*4)
    Elem = np.fromstring(data, dtype=np.float32)
    
    return Elem,total

def readFeaturesFile(filename):

    featurepoints =[(0,0)];
    
    txt = open(filename, "r")
    
    line = txt.readline()
    
    tokens = line.split()	
    featurepoints.remove((0,0))
    nopoints = int(tokens[0])
    for i in range(nopoints):
        featurepoints.append((tokens[3*i+1],int(tokens[3*i+2]),int(tokens[3*i+3])))

    #print featurepoints
    return nopoints,featurepoints	


